#include "location.hpp"

void localodomCallback(const nav_msgs::OdometryConstPtr &msg)
{
    current_point.x= msg->pose.pose.position.x;
    current_point.y= msg->pose.pose.position.y;
    current_point.z= msg->pose.pose.position.z;

    current_angle_Q = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x,
                                    msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    current_angle.x= msg->pose.pose.orientation.x;
    current_angle.y= msg->pose.pose.orientation.y;
    current_angle.z= msg->pose.pose.orientation.z;
    current_angle.w= msg->pose.pose.orientation.w;
    // curr_angle = toEulerAngle(current_angle);
    
}

void imuCallback(const sensor_msgs::Imu& msg)
{
    current_angle_Q = Eigen::Quaterniond(msg.orientation.w,msg.orientation.x,msg.orientation.y,msg.orientation.z);
}

void cloud1Callback(const sensor_msgs::PointCloud2ConstPtr& img) {
    //receive cloud message
    pcl::PointCloud<pcl::PointXYZI> latest_cloud1;
    pcl::fromROSMsg(*img, latest_cloud1);
    if (latest_cloud1.points.size() == 0)
    {   
        std::cout << "no cloud points" << std::endl;
        return;
    } 
    // Rpg means Rotation of lidar in ground coord
    // geometry_msgs::Quaternion lidar_angle_q_temp = tf:: createQuaternionMsgFromRollPitchYaw(lidar1_roll,lidar1_pitch,lidar1_yaw);
    // Eigen::Quaterniond lidar_angle_q1(lidar_angle_q_temp.w, lidar_angle_q_temp.x, lidar_angle_q_temp.y, lidar_angle_q_temp.z);
    // current_angle_Q = lidar_angle_q1;

    // pcl::PointCloud<pcl::PointXYZI> cloud_transform = transform_pc2map(latest_cloud1, current_angle_Q, lidar1_x, lidar1_y, lidar1_z);
    pcl::PointCloud<pcl::PointXYZI> cloud_transform = transform_pc2map(latest_cloud1, current_angle_Q, current_point.x, current_point.y, current_point.z);

    // cloud_transform.width = cloud_transform.points.size();
    // cloud_transform.height = 1;
    // cloud_transform.is_dense = true;
    // cloud_transform.header.frame_id = "map";
    // sensor_msgs::PointCloud2 cloud_msg;
    // pcl::toROSMsg(cloud_transform, cloud_msg);
    // map1_pub.publish(cloud_msg);
    //save the last  and last_last cloud 
    cloud1_last_last_last.swap(cloud1_last_last);
    cloud1_last_last.swap(cloud1_last);
    cloud1_last.swap(cloud1);
    cloud1.swap(cloud_transform);
}

void select_numCallback(const std_msgs::UInt8 &msg)
{
    select_id = msg.data;
}

void cloud_calc_callback(const ros::TimerEvent& /*event*/) 
{
    // if(current_point.z<1.0)
    // {
    //     std::cout<<"---->waiting for takeoff all vehicle....."<<std::endl;
    //     return;
    // }
    // std::cout<<"---->select_id:"<<select_id<<std::endl;
    for(int i = 0;i<id_list.size();i++)
    {
        if(ros::Time::now().toSec()-id_list[i].update_time>5.0)
        {
            std::cout<<"---->delete old box:"<<id_list[i].id<<std::endl;
            id_list.erase(id_list.begin() + i);
            // id_list[k].existed = false;
        }
    }
    pcl::PointCloud<pcl::PointXYZI> lidar_fusion1 = cloud1_last + cloud1;
    pcl::PointCloud<pcl::PointXYZI> lidar_fusion2 = lidar_fusion1+cloud1_last_last;
    pcl::PointCloud<pcl::PointXYZI> lidar_fusion3 = lidar_fusion2+cloud1_last_last_last;

    lidar_fusion3.width = lidar_fusion3.points.size();
    lidar_fusion3.height = 1;
    lidar_fusion3.is_dense = true;
    lidar_fusion3.header.frame_id = "map";
    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(lidar_fusion3, cloud_msg);
    map1_pub.publish(cloud_msg);
    pcl::PointCloud<pcl::PointXYZI>::Ptr global_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    global_cloud = lidar_fusion3.makeShared();

    if(global_cloud->points.size()>0)
    {
        // ROS_INFO("here calculate point cloud obstacle...");
        pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
        tree->setInputCloud(global_cloud);
        std::vector<pcl::PointCloud<pcl::PointXYZI>> clusters;//the cluster finnaly
        
        std::vector<pcl::PointIndices> clusterIndices;// create indices class
        pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec; //euclidean cluster class
        ec.setClusterTolerance(clusterTolerance);
        ec.setMinClusterSize(minsize);
        ec.setMaxClusterSize(maxsize);
        ec.setSearchMethod(tree);//use kd tree algorithm fo searching
        ec.setInputCloud(global_cloud); // feed point cloud
        ec.extract(clusterIndices); 
        
        for (pcl::PointIndices getIndices : clusterIndices)
        {
            pcl::PointCloud<pcl::PointXYZI> cloudCluster;
            // For each point indice in each cluster
            for (int index : getIndices.indices) 
            {
                cloudCluster.push_back(global_cloud->points[index]);
            }
            cloudCluster.width = cloudCluster.size();
            cloudCluster.height = 1;
            cloudCluster.is_dense = true;
            clusters.push_back(cloudCluster);
        }
        if(clusters.size()==0)
        {
            return;
        }

        int clusterId = 0;
        for (pcl::PointCloud<pcl::PointXYZI> cluster : clusters)
        {
            pcl::PointXYZI minPoint, maxPoint;
            pcl::getMinMax3D(cluster, minPoint, maxPoint); //get x,y,z min and max
            Box box_temp;
            box_temp.x_min = minPoint.x-(inflation);
            box_temp.y_min = minPoint.y-(inflation);
            box_temp.z_min = minPoint.z-inflation;
            box_temp.x_max = maxPoint.x+(inflation);
            box_temp.y_max = maxPoint.y+(inflation);
            box_temp.z_max = maxPoint.z+inflation;

            float box_center_x = (box_temp.x_min+box_temp.x_max)/2;
            float box_center_y = (box_temp.y_min+box_temp.y_max)/2;
            float box_center_z = (box_temp.z_min+box_temp.z_max)/2;

            if (box_center_x*box_center_x + box_center_y*box_center_y + box_center_z*box_center_z > 15*15)
                continue;
            

            // bool id_update_flag = false;
            // for(int k = 0;k<id_list.size();k++)
            // {
            //     // if(!id_list[k].existed)
            //     // {
            //     //     continue;
            //     // }
            //     double euclidean_dist = sqrt((box_center_x-id_list[k].x)*(box_center_x-id_list[k].x)
            //                                 +(box_center_y-id_list[k].y)*(box_center_y-id_list[k].y)
            //                                 +(box_center_x-id_list[k].z)*(box_center_x-id_list[k].z));
            //     if(euclidean_dist<track_search_field)
            //     {
            //         id_list[k].x = box_center_x;
            //         id_list[k].y = box_center_y;
            //         id_list[k].z = box_center_z;
            //         // id_list[k].existed = true;
            //         id_list[k].update_time = ros::Time::now().toSec();
            //         id_update_flag = true;
            //     }
            //     // if(ros::Time::now().toSec()-id_list[k].update_time>1.0)
            //     // {
            //     //     id_list.erase(id_list.begin() + k);
            //     //     // id_list[k].existed = false;
            //     // }
            // }
            // if(!id_update_flag)
            // {
            //     new_id++;
            //     Point_with_id new_point;
            //     new_point.id = new_id;
            //     new_point.x = box_center_x;
            //     new_point.y = box_center_y;
            //     new_point.z = box_center_z;
            //     // new_point.existed = true;
            //     new_point.update_time = ros::Time::now().toSec();
            //     id_list.push_back(new_point);
            // }

            Point_with_id new_point;
            new_point.id = clusterId;
            new_point.x = box_center_x;
            new_point.y = box_center_y;
            new_point.z = box_center_z;
            // new_point.existed = true;
            new_point.update_time = ros::Time::now().toSec();

            visualization_msgs::Marker cluster_marker = draw_cluster_marker(clusterId, box_temp);
            marker_pub.publish(cluster_marker);
            std::string str1 = "x:";
            std::string str2 = "\ny:";
            std::string str3 = "\nz:";
            std::string str4 = Convert(box_center_x);
            std::string str5 = Convert(box_center_y);
            std::string str6 = Convert(box_center_z);
            std::string coord = str1+str4+str2+str5+str3+str6;
            visualization_msgs::Marker cluster_id_marker = draw_cluster_id_marker(new_point, coord);
            marker_id_pub.publish(cluster_id_marker);
            
            std_msgs::String str_msg;
            std::string sss = fmt::format("id:{} x:{:.3f} y:{:.3f} z:{:.3f}", clusterId,box_center_x,box_center_y,box_center_z);
            str_msg.data = sss;
            str_pub.publish(str_msg);
            clusterId++;
        } 
        // float selected_x = 0.0;
        // float selected_y = 0.0;
        // float selected_z = 0.0;
        // bool selected_flag = false;
        
        // for (int j = 0;j<id_list.size();j++)
        // {
        //     // if(id_list[j].existed)
        //     // {
        //         std::string str1 = "x:";
        //         std::string str2 = "\ny:";
        //         std::string str3 = "\nz:";
        //         std::string str4 = Convert(id_list[j].x);
        //         std::string str5 = Convert(id_list[j].y);
        //         std::string str6 = Convert(id_list[j].z);
        //         std::string coord = str1+str4+str2+str5+str3+str6;
        //         visualization_msgs::Marker cluster_id_marker = draw_cluster_id_marker(id_list[j], coord);
        //         marker_id_pub.publish(cluster_id_marker);
        //     // }
        //     // if(select_id!=0 && id_list[j].id==select_id)
        //     // {
        //     //     selected_x = id_list[j].x;
        //     //     selected_y = id_list[j].y;
        //     //     selected_z = id_list[j].z;
        //     //     selected_flag = true;
        //     // }
        // }
        
        //judge condition:
        // if(select_id!=0&&selected_flag) //&& id_list[select_id].existed
        // {
        //     nav_msgs::Odometry plane_position;
        //     plane_position.header.stamp = ros::Time::now();
        //     plane_position.header.frame_id = "camera_odom_frame";
        //     plane_position.child_frame_id = "camera_pose_frame";
        //     std::cout<<"---->selected_xyz: "<<selected_x<<" , "<<selected_y<<" , "<<selected_z<<std::endl;

        //     plane_position.pose.pose.position.x = selected_x;
        //     plane_position.pose.pose.position.y = selected_y;
        //     plane_position.pose.pose.position.z = selected_z;
            // plane_pos_pub.publish(plane_position);

            //calculate the offset to center ray
            //test 


            // current_point.x = 0;
            // current_point.y = 0;
            // current_point.z = 0;
            
            // Eigen::Matrix3d Rpg_temp = current_angle_Q.toRotationMatrix();
            // //current R x vector
            // geometry_msgs::Vector3 x_vector;
            // x_vector.x = Rpg_temp(0,0);
            // x_vector.y = Rpg_temp(1,0);
            // x_vector.z = Rpg_temp(2,0);
            
            // //dist vector need to go
            // geometry_msgs::Vector3 x_dist_vector;
            // x_dist_vector.x = selected_x-current_point.x;
            // x_dist_vector.y = selected_y-current_point.y;
            // x_dist_vector.z = selected_z-current_point.z;
            

            // float x_vector_yaw = atan2(x_vector.y,x_vector.x);
            // float x_vector_pitch = atan2(x_vector.z,x_vector.x);

            // float x_dist_vector_yaw = atan2(x_dist_vector.y,x_dist_vector.x);
            // float x_dist_vector_pitch = atan2(x_dist_vector.z,x_dist_vector.x);

            // float yaw_error = x_dist_vector_yaw-x_vector_yaw;
            // float pitch_error = x_dist_vector_pitch-x_vector_pitch;

            // //calculate angle rate with p control
            // float yaw_rate = angle_rate_p_control(yaw_error*180/3.141593, 5,0,90);
            // float pitch_rate = angle_rate_p_control(pitch_error*180/3.141593, 10,0,90);

            // std_msgs::Float32MultiArray angle_rate2pub;
            // angle_rate2pub.data.push_back(yaw_rate);
            // angle_rate2pub.data.push_back(pitch_rate);
            // duoji_rate_pub.publish(angle_rate2pub);
            

        // }   
    }
    
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "ground_location");
    ros::NodeHandle nh;

    //read parameters from launch file
    nh.param<bool>("simulation",simulation,false);
    nh.param<double>("inflation",inflation,5.0);

    nh.param<double>("map_size_x",map_size_x,100.0);
    nh.param<double>("map_size_y",map_size_y,100.0);
    nh.param<double>("map_size_z_min",map_size_z_min,5.0);
    nh.param<double>("map_size_z_max",map_size_z_max,15.0);
    
    nh.param<double>("sample_size",sample_size,0.5);
    
    nh.param<double>("clusterTolerance",clusterTolerance,1.0);
    nh.param<int>("minsize",minsize,5);
    nh.param<int>("maxsize",maxsize,1000);

    nh.param<double>("min_lidar_distance",min_lidar_distance,2.0);
    nh.param<double>("max_lidar_distance",max_lidar_distance,100.0);
    nh.param<double>("lidar_intensity",lidar_intensity,50.0);
    nh.param<double>("track_search_field",track_search_field,1.0);
    

    nh.param<double>("lidar1_roll",lidar1_roll,0.0);
    nh.param<double>("lidar1_pitch",lidar1_pitch,0.0);
    nh.param<double>("lidar1_yaw",lidar1_yaw,0.0);
    nh.param<double>("lidar1_x",lidar1_x,0.0);
    nh.param<double>("lidar1_y",lidar1_y,0.0);
    nh.param<double>("lidar1_z",lidar1_z,0.0);


    nh.param<std::string>("lidar1_topic",lidar1_topic, "/rflysim/vehicle_0/lidar_0");


    cloud_pub_timer = nh.createTimer(ros::Duration(0.2), cloud_calc_callback);

    // lidar1_cloud_sub = nh.subscribe("/rflysim/vehicle_0/lidar_0",1,cloud1Callback);
    lidar1_cloud_sub = nh.subscribe(lidar1_topic,1,cloud1Callback);


    local_position_sub = nh.subscribe("/iris_0/mavros/local_position/odom",1,localodomCallback);
    // imu_sub = nh.subscribe("/mavros/imu/data",1,imuCallback);
    select_num_sub = nh.subscribe("/rviz/select_number_id",1,select_numCallback);
    

    marker_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_box", 1);
    marker_id_pub = nh.advertise<visualization_msgs::Marker>("/drone_id_marker", 1);
    map1_pub = nh.advertise<sensor_msgs::PointCloud2>("/pcl/cloud1", 1);
    str_pub = nh.advertise<std_msgs::String>("/mav_pose_str", 1);


    // plane_pos_pub = nh.advertise<nav_msgs::Odometry>("/pcl/odom_to_serial", 1);

    // duoji_rate_pub = nh.advertise<std_msgs::Float32MultiArray>("/destination_servo_angle",1);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(100);    
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
 
    return 0;
}


pcl::PointCloud<pcl::PointXYZI> transform_pc2map(pcl::PointCloud<pcl::PointXYZI> latest_cloud, Eigen::Quaterniond lidar_angle_q,
                    double lidar_x, double lidar_y, double lidar_z)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>);
    cloud_in = latest_cloud.makeShared();
    //sample to voxel grid 
    pcl::PCLPointCloud2::Ptr cloud_in2 (new pcl::PCLPointCloud2 ());
	pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
	toPCLPointCloud2(*cloud_in,*cloud_in2);
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud (cloud_in2);
	sor.setLeafSize(sample_size,sample_size,sample_size);
	sor.filter (*cloud_filtered);
	fromPCLPointCloud2(*cloud_filtered,*cloud_in);
    
    pcl::PointXYZI pt;
    // pitch 90
    Eigen::Matrix3d Rpitch;
    Rpitch<< 0, 0, 1,
        0, 1, 0,
        -1, 0, 0;
    // roll 90
    Eigen::Matrix3d Rroll;
    Rroll<< 1, 0, 0,
            0, 0, 1,
            0, -1, 0;
    
    Eigen::Matrix3d Rpg = lidar_angle_q.toRotationMatrix();
    pcl::PointCloud<pcl::PointXYZI> cloud_out;
    
    for (size_t i = 0; i < cloud_in->points.size(); ++i) 
    {
        if (!pcl::isFinite(cloud_in->points[i]))
        {
            continue;
        }
        //delete points too near to lidar ,such as itself
        double dist_body = sqrt(cloud_in->points[i].x*cloud_in->points[i].x+cloud_in->points[i].y*cloud_in->points[i].y+cloud_in->points[i].z*cloud_in->points[i].z);
        if(abs(dist_body)<min_lidar_distance || abs(dist_body)>max_lidar_distance)
        {
            continue;
        }
        // if(!simulation)
        // {
        //     if(cloud_in->points[i].intensity<lidar_intensity)
        //     {
        //         continue;
        //     }
        // }

        Eigen::Vector3d PointE(cloud_in->points[i].x,cloud_in->points[i].y,cloud_in->points[i].z);
        //rotate pitch and roll 90 degree because the coordinate differences if in sumilation
        if(simulation)
        {
            PointE =  Rpitch*PointE;
            PointE =  Rroll*PointE;
        }
        
        //rotate the point cloud to the vehicle coordinate
        PointE =  Rpg*PointE;
        //final_point = Rotation*origin_point + Transform
        pt.x = PointE(0) + lidar_x;
        pt.y = PointE(1) + lidar_y;
        pt.z = PointE(2) + lidar_z;
        //judge what type of points push_back to cloud:temp x,y~(trajectory) , z~(3,10)
        if(pt.z>map_size_z_min && pt.z<map_size_z_max && abs(pt.x)<map_size_x && abs(pt.y)<map_size_y) 
        {
            cloud_out.push_back(pt);    
        }  
    }
    return cloud_out;
}

visualization_msgs::Marker draw_cluster_marker(int clusterId, Box box)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "target_box";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = clusterId; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    // line_strip.lifetime = ros::Duration(0.1);
    line_strip.scale.x = 0.1; //width of the line
    line_strip.color.r = 0.0; line_strip.color.g = 0.5; line_strip.color.b = 0.5;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs:: Point p[8];
    p[0].x = box.x_min;     p[0].y = box.y_max;     p[0].z = box.z_max;
    p[1].x = box.x_min;     p[1].y = box.y_min;     p[1].z = box.z_max;
    p[2].x = box.x_min;     p[2].y = box.y_min;     p[2].z = box.z_min;
    p[3].x = box.x_min;     p[3].y = box.y_max;     p[3].z = box.z_min;
    p[4].x = box.x_max;     p[4].y = box.y_max;     p[4].z = box.z_min;
    p[5].x = box.x_max;     p[5].y = box.y_min;     p[5].z = box.z_min;
    p[6].x = box.x_max;     p[6].y = box.y_min;     p[6].z = box.z_max;
    p[7].x = box.x_max;     p[7].y = box.y_max;     p[7].z = box.z_max;
    //LINE_STRIP link 0-1 1-2 2-3 only
    for (int i = 0; i < 8; i++){
        line_strip.points.push_back(p[i]);
    }
    //so we need to add every links mannualy
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[3]);
    line_strip.points.push_back(p[2]);
    line_strip.points.push_back(p[5]);
    line_strip.points.push_back(p[6]);
    line_strip.points.push_back(p[1]);
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[7]);
    line_strip.points.push_back(p[4]);
    
    return line_strip;
    
    //end draw
}

visualization_msgs::Marker draw_cluster_id_marker(Point_with_id id_point, std::string point_coord) 
{
    visualization_msgs::Marker marker;
    marker.header.frame_id="map";
    marker.header.stamp = ros::Time::now();
    marker.ns = "basic_shapes";
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1.0;
    marker.id =id_point.id;
    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    // marker.lifetime = ros::Duration(0.1);
    marker.scale.z = 0.3;
    marker.color.b = 0;
    marker.color.g = 0;
    marker.color.r = 255;
    marker.color.a = 1;
    // if(id_point.id==select_id)
    // {
    //     marker.scale.z = 0.5;
    //     marker.color.b = 0;
    //     marker.color.g = 255;
    //     marker.color.r = 255;
    //     marker.color.a = 1;
    // }
    // else{
    //     marker.scale.z = 0.3;
    //     marker.color.b = 0;
    //     marker.color.g = 0;
    //     marker.color.r = 255;
    //     marker.color.a = 1;
    // }
    
    

    geometry_msgs::Pose pose_name;
    pose_name.position.x =  id_point.x+0.5;
    pose_name.position.y =  id_point.y+0.5;
    pose_name.position.z =  id_point.z+0.5;
    // std::ostringstream str;
    // str<<id_point.id;
    // marker.text=str.str();
    marker.text=point_coord;
    
    marker.pose=pose_name;

    return marker;

}

float angle_rate_p_control(float angle_error, float P, float dead_zone, float angle_rate_limit)
{
    float out_put_rate=0.0;
    if (angle_error < -180)
        angle_error += 360;
    else if (angle_error > 180)
        angle_error -= 360;
    out_put_rate = P * angle_error;

    if (abs(angle_error) <= dead_zone) {
        out_put_rate = 0.0;
    }
    if (abs(out_put_rate) >= angle_rate_limit) {
        out_put_rate = limit(out_put_rate, angle_rate_limit);
    } 
    return out_put_rate;
}
        
std::string Convert(float Num)
{
    std::ostringstream oss;
    oss<<Num;
    std::string str(oss.str());
    return str;
}